#include "atomic_0826_navigation_calculation_service.h"

// 简单模拟导航解算实现
int getNavSolution(NavSolutionInput *input, NavSolutionOutput *output) {
    // 检查输入输出指针有效性
    if (input == nullptr || output == nullptr) {
        return ATOMIC_SERVICE_ERROR;
    }

    // 初始化输出结构体（避免未初始化值）
    memset(output, 0, sizeof(NavSolutionOutput));

    // 模拟解算有效性：如果GPS或IMU有一个有效，则认为解算有效
    output->solution_valid = (input->gps_valid || input->imu_valid) ? 1 : 0;

    // 解算时间戳使用输入的同步时间戳
    output->sol_time = input->sync_time;
    output->timestamp = input->sync_time;

    // 模拟位置信息（GPS弧度转度，加微小噪声）
    std::random_device rd;
    std::mt19937 gen(rd());
    std::normal_distribution<> pos_noise(0, 0.5); // 位置噪声（均值0，标准差0.5）
    
    output->latitude = RAD_TO_DEG(input->gps_lat_rad) + pos_noise(gen);
    output->longitude = RAD_TO_DEG(input->gps_lon_rad) + pos_noise(gen);
    output->altitude = input->gps_alt + (float)pos_noise(gen);

    // 位置标准差模拟
    output->pos_std_lat = 0.3f;
    output->pos_std_lon = 0.3f;
    output->pos_std_alt = 0.5f;

    // 模拟速度信息（基于里程计，分配到北向为主）
    std::normal_distribution<> vel_noise(0, 0.1); // 速度噪声
    output->velocity_north = input->odom_valid ? (input->odom_speed + vel_noise(gen)) : 0.0f;
    output->velocity_east = (float)vel_noise(gen);
    output->velocity_up = (float)vel_noise(gen) * 0.5f;

    // 速度标准差模拟
    output->vel_std_east = 0.1f;
    output->vel_std_north = 0.1f;
    output->vel_std_up = 0.2f;

    // 模拟姿态信息（固定小角度加噪声）
    std::normal_distribution<> att_noise(0, 0.2); // 姿态噪声
    output->attitude_roll = 0.5f + (float)att_noise(gen);
    output->attitude_pitch = 0.3f + (float)att_noise(gen);
    output->attitude_heading = 30.0f + (float)att_noise(gen) * 5; // 航向角范围稍大

    // 姿态标准差模拟
    output->att_std_roll = 0.2f;
    output->att_std_pitch = 0.2f;
    output->att_std_heading = 0.5f;

    // 其他字段模拟
    output->return_type = ATOMIC_SERVICE_SUCCESS;
    output->sensor_id = 1; // 模拟传感器ID
    output->raw_error = 0.001; // 模拟原始误差
    output->position_x = output->longitude * 1000; // 简单映射
    output->position_y = output->latitude * 1000;  // 简单映射

    return ATOMIC_SERVICE_SUCCESS;
}